;
AREA ?C?SWI, CODE, READONLY, ALIGN=2

PUBLIC SWI_Handler?A, ?SWI?Table, ?SWI?Empty

SWI_Handler?A    PROC CODE32
        STMFD   SP!,{LR}           ; Store LR register
        MRS     R8,SPSR
        TST     R8,#0x20           ; SWI call from 
        LDRNEH  R8,[LR,#-2]        ; Thumb: Load halfword instruction
        ANDNE   R8,R8,#0xFF        ;        extract SWI number
        LDREQ   R8,[LR,#-4]        ; ARM:   Load word     instruction
        BICEQ   R8,R8,#0xFF000000  ;        extract SWI number
                                   ; R4 now contains SWI number
; SWI Handler
;       LDR     R12,[PC,#(?SWI?Table-$-8)] ; Maximum number of interrupts
;       LDR     R12,[PC,#0x24]     ; Maximum number of interrupts
        ADR     R12,?SWI?Table
        LDR     R12,[R12]          ; load last SWI-Function-number

        CMP     R8,R12                                             
        BGT     ?SWI?Empty         ; overflow
        ADR     R12,?SWI?Table+4
        LDR     R12,[R12,R8,LSL #2]; SWI function address
        MOV     LR,PC              ; Return address
        BX      R12                ; Call SWI function

        LDMFD   SP!,{PC}^          ; Return

?SWI?Empty:
        B       $                  ; no existing SWI

; *** DO NOT MODIFY THIS PORTION OF THE FILE ***
?SWI?Table:                        ; Marker for LA Linker                       
; 
; The LA Linker inserts at this label
;       DD      0                  ; <last SWI function number>
;       DD      ?SWI?Empty         ; <entry for SWI function 0>
;       DD      <SWI entry 1>
;       DD      <SWI entry 2>
;       DD           :
; For non-existing SWI functions DD ?SWI?Empty is inserted

        ENDP

        END
